%% Move robot
% Contains main loop to move the robot

global workspace_data d_min_pos_last SP record_data c_vel_pos speed_init timestep obst_det error;

% initialize serial port and robot
% SP = serial_port_start;
% pioneer_init(SP);
% pioneer_reset_odometry(SP);
% pause(2);

% Initialize Kalman filter
Kalman_param.kl = 1e-3;
Kalman_param.kr = 1e-3;
Kalman_param.B  = 0.5;
Kalman_param.H = eye(3);
Kalman_param.F = eye(3);

% x_hat_old = zeros(3,1); % prediction 
x_hat_old = [workspace_data.x_init;workspace_data.y_init;workspace_data.theta];

d = speed_init*timestep;
R_old  = zeros(3);
% R_old,Kalman_param.kl,Kalman_param.kr,Kalman_param.B,d
R_old = line_cov(R_old,Kalman_param.kl,Kalman_param.kr,Kalman_param.B,d);
P = inv(Kalman_param.H)*R_old*inv(Kalman_param.H');

% Initialize robot position
r_ref = [workspace_data.x_init workspace_data.y_init];
record_data.r_ref = r_ref;
r = r_ref;

omega = 0; % correction angle 
phi = 0; % correction angle
obst_det = 0;
eta_n = 0;

theta = workspace_data.theta;

% workspace_data

% counters
n_A = 1;
n_C = 1;

a_SEEK = [ 0 0 ];
v_SEEK = [ 0 0 ];

% Initialize movement loop
error = 0;
done = 0;
loop_ind = 1;


% generate trajectory if not done before
if ~isfield(workspace_data,'traj_px')
    gen_path_traj;
    % output info
    set(handles.txt_workspace_output,'String',[get(handles.txt_workspace_output,'String');{'Path and trajectory calculated.'}]);
end

% initializing record structure
record_data.flag_Kalman = flag_Kalman;
record_data.sonar_data = zeros(1,8);
record_data.r = zeros(1,2);
record_data.v = zeros(1,2);

p_i = 1;
target = p_i + (fix(timestep*speed_init/workspace_data.px_to_m_x*c_vel_pos) + 1);
p_f    = 1;

% speed_init
v = [ cos(theta), sin(theta) ]*speed_init;
% d = speed_init*timestep;

r = (r+v*timestep)';
r = r';

% theta = theta + omega * timestep;
% v = [ cos(theta), sin(theta) ]*speed_init;


% p_i
% target
% p_f
% v
% d
% r
% theta
% eta_n
% omega
% phi

% first calculation of values for trajectory following
[ r_F, eta_n, p_i, target, p_f] = transf_points_for_robot( v, r, p_i, target, p_f, eta_n);
pioneer_set_controls(SP,round(speed_init*1e3),0);       
pause(timestep);

loop_ind = loop_ind + 1;
h_rob_move = [];

% aim_reached = 0;

% did the robot already reach the aim within tolerance?
aim_reached = (norm(workspace_data.traj_m(end,:) - r) < d_min_pos_last);

% JUST FOR CHECK
% read odometry data from robot
% ch_odometry = pioneer_read_odometry();
% ch_r_x = ch_odometry(1); 
% ch_r_y = ch_odometry(2); 
% ch_theta = ch_odometry(3);
% 
% ch_r_x = ch_r_x/1e3;
% ch_r_y = ch_r_y/1e3;
% ch_theta = ch_theta*pi/2048;

while ~aim_reached && ~error  % movement loop
    
    % avoid collisions with obstacles using sonar
    if flag_avoid_obst
        [omega, sonar_data , n_A] = collision_avoidance(n_A, omega);
        record_data.sonar_data(loop_ind,:) = sonar_data;

        % plot current sonar data
        sonar_plot(handles.ax_sonar,sonar_data);
        obst_det
    end
%     omega1 = omega
    
    % correct robot movement to keep it on trajectory
    [omega, phi, n_C] = correct_rob_movement(r_F, p_f, eta_n, n_C, target, r, v, omega);
      
    record_data.omega(loop_ind) = omega;
    record_data.phi(loop_ind) = radtodeg(phi);
%         omega2 = omega

    % move robot
    tic;
    pioneer_set_controls(SP,round(speed_init*1e3),round(omega));       
    pause(timestep); 
    cur_time = toc;
    record_data.cur_time(loop_ind) = cur_time;

    % localize the robot (incl. Kalman filter)                                                   
    [r,theta,v,P_cur,R_cur,x_hat_cur] = localize_robot(r_ref,speed_init,x_hat_old,cur_time,omega,P,workspace_data.theta,Kalman_param,R_old,flag_Kalman,loop_ind);
    record_data.r(loop_ind,:) = r;
    record_data.theta(loop_ind) = rad2deg(theta);
    record_data.v(loop_ind,:) = v;
%     r
%     disp(radtodeg(theta))
    v = [ cos(theta), sin(theta) ]*speed_init;
    
    % plot current robot position and orientation
    h_rob_move = plot_robot_movement(handles.ax_workspace,h_rob_move,r,theta,25);
    
    % update values for trajectory following
    [ r_F, eta_n, p_i, target, p_f] = transf_points_for_robot(v, r, p_i, target, p_f, eta_n);
    
    % update loop
    loop_ind = loop_ind + 1;
    R_old = R_cur;
    P = P_cur;
    x_hat_old = x_hat_cur;
    record_data.cur_time(loop_ind) = cur_time;

    % check if target position is (roughly) reached
    aim_reached = (norm(workspace_data.traj_m(end,:) - r) < d_min_pos_last);
    
    error = ~isvalid(SP); 
    
%     [ r_F, eta_n, v, d, r, p_i, target, p_f] = transf_points_for_robot(v, d, r, p_i, target, p_f, eta_n);
%     
%     % avoid collisions with obstacles using sonar
%     [omega, phi, obst_det, sonar_data , n_A] = collision_avoidance(n_A, omega, phi);
%     record_data.omega(loop_ind) = omega;
%     record_data.phi(loop_ind) = radtodeg(phi);
%     record_data.sonar_data(loop_ind,:) = sonar_data;
% 
%     % correct robot movement to keep it on trajectory
%     [omega, phi, n_C, a_SEEK, v_SEEK] = correct_rob_movement(r_F, p_f, eta_n, n_C, obst_det ,target, r, v, a_SEEK, v_SEEK, omega, phi);
% 
%     % plot current sonar data
% %     h_sonar_plot = sonar_plot(handles.ax_sonar,sonar_data);
%     % drawnow
%     
%     % move robot
%     tic;    
% %     pioneer_set_controls(SP,round(speed_init*1e3),round(omega));       
% %     pause(timestep); 
%     cur_time = toc;
%     record_data.cur_time(loop_ind) = cur_time;
%     
%     % plot
% %     plot_robot_movement( input_args )
%     
%     % localize the robot (incl. Kalman filter)
%     [r,theta,v,R_cur] = localize_robot(r_ref,speed_init,x_hat_old,cur_time,omega,P,theta_init,Kalman_param,R_old,flag_Kalman);
%     record_data.r(loop_ind,:) = r;
%     record_data.theta(loop_ind) = rad2deg(theta);
%     record_data.v(loop_ind,:) = v;
%     
%     % update loop
%     loop_ind = loop_ind + 1;
%     R_old = R_cur;
%     
%     n_CICLE = n_CICLE + 1;
% 
%     % check if target position is (roughly) reached
%     if norm(workspace_data.traj_m(end,:) - r) < d_min_pos_last;
%         break
%     end    

end

save(['WORKSPACE' datestr(now,30)]);

% stop robot when aim is reached
if ~isa(SP, 'double')
    if isvalid(SP) 
        pioneer_set_controls(SP, 0, 0);
        pioneer_close(SP);
        set(handles.txt_workspace_output,'Visible','On');
        set(handles.txt_workspace_output,'String',[get(handles.txt_workspace_output,'String');{'Robot STOPPED.'}]);
        return;
    end
end